/*==============================================================================
             Copyright (c) 2009 - All rights reserved
================================================================================
File description:

    Math Utility function definition

================================================================================
      Date            Name                    Description of Change
08-Sep-2009  Xiaofeng.Gu                    Written
$HISTORY$
==============================================================================*/
#include "stdafx.h"
#include "MathUtil.h"
#include "math.h"
#include "Vec.h"

#ifdef _DEBUG
#define new DEBUG_NEW
#endif

bool CMathUtil::CalRayToBallPt(double Ray[6], double Ball[4], double Pt[3])
{
	double x0, x1, x2, y0, y1, y2, z0, z1, z2;
	x1 = Ray[0];
	y1 = Ray[1];
	z1 = Ray[2];
	x2 = Ray[3];
	y2 = Ray[4];
	z2 = Ray[5];
	x0 = Ball[0];
	y0 = Ball[1];
	z0 = Ball[2];
	double A = x1 - x0; double B = x2 - x1;
	double C = y1 - y0; double D = y2 - y1;
	double E = z1 - z0; double F = z2 - z1;
	double R = Ball[3];
	double G = A * A + C * C + E * E - R * R;
	double H = 2 * (A * B + C * D + E * F);
	double I = B * B + D * D + F * F;
	double W = H * H - 4 * I * G;

	if (W < 0)
	{
		double BallCenter[3];
		BallCenter[0] = x0;
		BallCenter[1] = y0;
		BallCenter[2] = z0;
		CalPtToLineClosestPt(BallCenter, Ray, Pt);
		return false;
	}

	if (fabs(W) < DELTA)
	{
		double u = (- H + sqrt(W)) / (2 * I);
		Pt[0] = x1 + (x2 - x1) * u;
		Pt[1] = y1 + (y2 - y1) * u;
		Pt[2] = z1 + (z2 - z1) * u;
	}
	else
	{
		double u1 = (- H + sqrt(W)) / (2 * I);
		double u2 = (- H - sqrt(W)) / (2 * I);
		if (u1 * u1 < u2 * u2)
		{
			Pt[0] = x1 + (x2 - x1) * u1;
			Pt[1] = y1 + (y2 - y1) * u1;
			Pt[2] = z1 + (z2 - z1) * u1;
		}
		else
		{
			Pt[0] = x1 + (x2 - x1) * u2;
			Pt[1] = y1 + (y2 - y1) * u2;
			Pt[2] = z1 + (z2 - z1) * u2;
		}
	}

	return true;
}

void CMathUtil::CalPtToLineClosestPt(double Pt[3], double Line[6], double ClosestPt[3])
{
	CVec V = CVec(Line[3] - Line[0], Line[4] - Line[1], Line[5] - Line[2]);
	V.Identify();

	CVec V2 = CVec(Pt[0] - Line[0], Pt[1] - Line[1], Pt[2] - Line[2]);

	double dis = V * V2;

	ClosestPt[0] = Line[0] + V[0] * dis;
	ClosestPt[1] = Line[1] + V[1] * dis;
	ClosestPt[2] = Line[2] + V[2] * dis;
}